#include "Header.h"

/*电机设置速度（占空比）*/
void Motor_SetSpeed(int8_t speed_left, int8_t speed_right)
{
    if (speed_left == 0)
    {
        GPIO_ResetBits(GPIOA, AIN1 | AIN2); // 停止电机
        PWM_PWMAChangeDuty(0); // 停止 PWM 输出
    }
    else if (speed_left > 0 && speed_left <= 100)
    {
        GPIO_SetBits(GPIOA, AIN1);  // 设置正转
        GPIO_ResetBits(GPIOA, AIN2);
        PWM_PWMAChangeDuty(speed_left); // 调整 PWM 占空比，控制转速
    }
    else if (speed_left > 100)
    {
        GPIO_SetBits(GPIOA, AIN1);  // 设置正转
        GPIO_ResetBits(GPIOA, AIN2);
        PWM_PWMAChangeDuty(100); // 调整 PWM 占空比，控制转速
    }
    else if (speed_left < 0 && speed_left >= -100)
    {
        GPIO_SetBits(GPIOA, AIN2);  // 设置反转
        GPIO_ResetBits(GPIOA, AIN1);
        PWM_PWMAChangeDuty(-speed_left); // 控制反转占空比
    }
    else if (speed_left < -100)
    {
        GPIO_SetBits(GPIOA, AIN2);  // 设置反转
        GPIO_ResetBits(GPIOA, AIN1);
        PWM_PWMAChangeDuty(100); // 控制反转占空比
    }
    if (speed_right == 0)
    {
        GPIO_ResetBits(GPIOA, BIN1 | BIN2); // 停止电机
        PWM_PWMBChangeDuty(0); // 停止 PWM 输出
    }
    else if (speed_right > 0 && speed_right <= 100)
    {
        GPIO_SetBits(GPIOA, BIN1);  // 设置正转
        GPIO_ResetBits(GPIOA, BIN2);
        PWM_PWMBChangeDuty(speed_right);
    }
    else if (speed_right > 100)
    {
        GPIO_SetBits(GPIOA, BIN1);  // 设置正转
        GPIO_ResetBits(GPIOA, BIN2);
        PWM_PWMBChangeDuty(100);
    }
    else if (speed_right < 0 && speed_right >= -100)
    {
        GPIO_SetBits(GPIOA, BIN2);  // 设置反转
        GPIO_ResetBits(GPIOA, BIN1);
        PWM_PWMBChangeDuty(-speed_right); // 控制反转占空比
    }
    else if (speed_right < -100)
    {
        GPIO_SetBits(GPIOA, BIN2);  // 设置反转
        GPIO_ResetBits(GPIOA, BIN1);
        PWM_PWMBChangeDuty(100); // 控制反转占空比
    }
}

/*电机停转*/
void Motor_Stop(void)
{
    GPIO_ResetBits(GPIOA, STBY);
}

/*电机初始化*/
void Motor_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = AIN1 | AIN2 | BIN1 | BIN2 | STBY;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_SetBits(GPIOA, STBY);
    PWM_Init();
    Encoder_Init();
}

/*控制小车转过90度*/
void Motor_Turn90(enum Direction d)
{
    float target;
    if(d == Left)
    {
        target = yaw + 90;
    }
    else 
    {
        target = yaw - 90;
    }
    char message[50];
    
    PID_Structure pid_AngleStrucuture;
    PID_Structure pid_LWheelStructure; 
    PID_Structure pid_RWheelStructure;
    Motor_SetSpeed(0,0);
    Delay_ms(100);
    do
    {
        PID_StructureInit(&pid_AngleStrucuture, 10, 0, 2);
        PID_Target_Cal(&pid_AngleStrucuture, yaw, target);
        float SpeedTargetLeft = -pid_AngleStrucuture.out;
        float SpeedTargetRight = pid_AngleStrucuture.out;

        PID_StructureInit(&pid_LWheelStructure, 0.04, 0, 0);
        PID_StructureInit(&pid_RWheelStructure, 0.04, 0, 0);
        PID_Increase_Cal(&pid_LWheelStructure, Encoder_Speed1, SpeedTargetLeft);
        PID_Increase_Cal(&pid_RWheelStructure, Encoder_Speed2, SpeedTargetRight);

        Motor_SetSpeed(pid_LWheelStructure.out, pid_RWheelStructure.out);
        sprintf(message, "target:%f,yaw:%f\r\n", target, yaw);
        HC05_SendString(message);
        Delay_ms(20);
    }
    while(abs(target - yaw) > 5);
    yaw = 0;
}
